#ifndef STEPPERMOTOR_H
#define STEPPERMOTOR_H
#include "TimerEventQueue.h"
#define MINSTEPTIME 20000
#define PINSNUMBER 4
class StepperMotor : public TimerEventListener
{
public:
    StepperMotor(TimerQueue& ptimer,
                 unsigned short ppin1, unsigned short ppin2,
                 unsigned short ppin3, unsigned short ppin4, bool phalfstep = false)
        :timer(ptimer), halfStep(phalfstep)
    {
        pins[0] = ppin1;
        pins[1] = ppin2;
        pins[2] = ppin3;
        pins[3] = ppin4;
        for(int i=0;i<PINSNUMBER;++i)
          pinMode(pins[i], OUTPUT);
        fullStepCount = 0;
		runforward = true;

    }
    void start(unsigned short speedPercentage=0)
    {
        if(speedPercentage !=0)
            stepTime = (100./speedPercentage) * MINSTEPTIME;
        timer.registerTimerEvent(this, stepTime);
    }
    void stopMotor()
    {
//        Serial.println("StopMotor");
        stepTime = 0;
    }
	void forward(){runforward = true;}
	void backward(){runforward = false;}
    virtual long unsigned onTime()
    {
        step();
        return stepTime;
    }
    void setMotorSpeed(unsigned short speedPercentage)
    {
        
      if(speedPercentage != 0)
      {
        if(stepTime == 0)
          start(speedPercentage);
        else
          stepTime = (100./speedPercentage) * MINSTEPTIME;// (speedPercentage * MINSTEPTIME)/100;
      }
      else 
        stopMotor();
 //     Serial.println(stepTime);
    }
private:
    void step()
    {
        if(halfStep)
            doHalfStep();
        else
            doFullStep();
    }
    void doFullStep()
    {
        if(runforward)
		fullStepCount = (fullStepCount + 1) % 4;
	else
		fullStepCount = (fullStepCount + PINSNUMBER-1) % 4;
        digitalWrite(pins[0], fullStepCount == 0 ? HIGH :LOW);
        digitalWrite(pins[1], fullStepCount == 1 ? HIGH :LOW);
        digitalWrite(pins[2], fullStepCount == 2 ? HIGH :LOW);
        digitalWrite(pins[3], fullStepCount == 3 ? HIGH :LOW);
    }
    void doHalfStep()
    {
	if (runforward)
		halfstepCount = (halfstepCount+1)%(PINSNUMBER*2);
	else
		halfstepCount = (halfstepCount+(PINSNUMBER*2-1))%(PINSNUMBER*2);
        digitalWrite(pins[0], halfstepCount == 0 || halfstepCount == 1 || halfstepCount == 7  ? HIGH :LOW);
        digitalWrite(pins[1], halfstepCount == 1 || halfstepCount == 2 || halfstepCount == 3  ? HIGH :LOW);
        digitalWrite(pins[2], halfstepCount == 3 || halfstepCount == 4 || halfstepCount == 5  ? HIGH :LOW);
        digitalWrite(pins[3], halfstepCount == 5 || halfstepCount == 6 || halfstepCount == 7  ? HIGH :LOW);
        

    }
    TimerQueue& timer;
    unsigned short pins[PINSNUMBER];
    unsigned short fullStepCount;
    unsigned long stepTime;
    unsigned short halfstepCount;
    bool runforward;
    bool halfStep;
};

#endif // STEPPERMOTOR_H
